#ifndef _OBERON7_HPP_
#define _OBERON7_HPP_

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

using namespace std_msgs::msg;
using namespace sensor_msgs::msg;

namespace oberon7_robot_driver
{

  class Oberon7Driver : public rclcpp::Node
  {
  private:
    std::vector<double> _joint_pos;
    std::vector<double> _joint_vel;
    std::vector<double> _joint_eff;

    rclcpp::Subscription<JointState>::SharedPtr _state_sub;
    rclcpp::Publisher<Float64>::SharedPtr _azimuth_pub;
    rclcpp::Publisher<Float64>::SharedPtr _shoulder_pub;
    rclcpp::Publisher<Float64>::SharedPtr _elbow_pub;
    rclcpp::Publisher<Float64>::SharedPtr _roll_pub;
    rclcpp::Publisher<Float64>::SharedPtr _pitch_pub;
    rclcpp::Publisher<Float64>::SharedPtr _wrist_pub;

    void JointStatesCallback(const JointState::SharedPtr msg);

  public:
    Oberon7Driver();
    void SendJointCommand(std::vector<double> joint_cmd);
    std::vector<double> GetJointPosition();
    std::vector<double> GetJointVelocity();
    std::vector<double> GetJointEffort();
  };

}

#endif // _OBERON7_HPP_